#include <esp_log.h>
#include <esp_err.h>
#include <nvs.h>
#include <nvs_flash.h>
#include <driver/gpio.h>
#include <esp_event.h>

#include "application.h"
#include "system_info.h"

#include "dht.h"

#include <string.h>

#include "driver/ledc.h"

#include "freertos/task.h"

#include "esp_timer.h"

#include "driver/i2c.h"

#include "driver/i2c_master.h"

//#include "i2c_device.h"

#include "freertos/FreeRTOS.h"

#include <cassert>

#define TAG "main"

//DHT22区域
#define DHT_TIMER_INTERVAL 2
#define DHT_DATA_BITS 40
#define DHT_DATA_BYTES (DHT_DATA_BITS / 8)

static portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;
#define PORT_ENTER_CRITICAL() portENTER_CRITICAL(&mux)
#define PORT_EXIT_CRITICAL() portEXIT_CRITICAL(&mux)

#define CHECK_ARG(VAL) do { if (!(VAL)) return ESP_ERR_INVALID_ARG; } while (0)

#define CHECK_LOGE(x, msg, ...) do { \
        esp_err_t __; \
        if ((__ = x) != ESP_OK) { \
            PORT_EXIT_CRITICAL(); \
            ESP_LOGE(TAG, msg, ## __VA_ARGS__); \
            return __; \
        } \
    } while (0)

static esp_err_t dht_await_pin_state(gpio_num_t pin, uint32_t timeout,
        int expected_pin_state, uint32_t *duration)
 {
     /* XXX dht_await_pin_state() should save pin direction and restore
      * the direction before return. however, the SDK does not provide
      * gpio_get_direction().
      */
     gpio_set_direction(pin, GPIO_MODE_INPUT);
     for (uint32_t i = 0; i < timeout; i += DHT_TIMER_INTERVAL)
     {
         // need to wait at least a single interval to prevent reading a jitter
         esp_rom_delay_us(DHT_TIMER_INTERVAL);
         if (gpio_get_level(pin) == expected_pin_state)
         {
             if (duration)
                 *duration = i;
             return ESP_OK;
         }
     }
 
     return ESP_ERR_TIMEOUT;
 }

 static inline esp_err_t dht_fetch_data(dht_sensor_type_t sensor_type, gpio_num_t pin, uint8_t data[DHT_DATA_BYTES])
{
    uint32_t low_duration;
    uint32_t high_duration;

    // Phase 'A' pulling signal low to initiate read sequence
    gpio_set_direction(pin, GPIO_MODE_OUTPUT_OD);
    gpio_set_level(pin, 0);
    esp_rom_delay_us(sensor_type == DHT_TYPE_SI7021 ? 500 : 20000);
    gpio_set_level(pin, 1);

    // Step through Phase 'B', 40us
    CHECK_LOGE(dht_await_pin_state(pin, 40, 0, NULL),
            "Initialization error, problem in phase 'B'");
    // Step through Phase 'C', 88us
    CHECK_LOGE(dht_await_pin_state(pin, 88, 1, NULL),
            "Initialization error, problem in phase 'C'");
    // Step through Phase 'D', 88us
    CHECK_LOGE(dht_await_pin_state(pin, 88, 0, NULL),
            "Initialization error, problem in phase 'D'");

    // Read in each of the 40 bits of data...
    for (int i = 0; i < DHT_DATA_BITS; i++)
    {
        CHECK_LOGE(dht_await_pin_state(pin, 65, 1, &low_duration),
                "LOW bit timeout");
        CHECK_LOGE(dht_await_pin_state(pin, 75, 0, &high_duration),
                "HIGH bit timeout");

        uint8_t b = i / 8;
        uint8_t m = i % 8;
        if (!m)
            data[b] = 0;

        data[b] |= (high_duration > low_duration) << (7 - m);
    }

    return ESP_OK;
}

static inline int16_t dht_convert_data(dht_sensor_type_t sensor_type, uint8_t msb, uint8_t lsb)
{
    int16_t data;

    if (sensor_type == DHT_TYPE_DHT11)
    {
        data = msb * 10;
    }
    else
    {
        data = msb & 0x7F;
        data <<= 8;
        data |= lsb;
        if (msb & BIT(7))
            data = -data;       // convert it to negative
    }

    return data;
}

esp_err_t dht_read_data(dht_sensor_type_t sensor_type, gpio_num_t pin,
    int16_t *humidity, int16_t *temperature)
{
CHECK_ARG(humidity || temperature);

uint8_t data[DHT_DATA_BYTES] = { 0 };

gpio_set_direction(pin, GPIO_MODE_OUTPUT_OD);
gpio_set_level(pin, 1);

PORT_ENTER_CRITICAL();
esp_err_t result = dht_fetch_data(sensor_type, pin, data);
if (result == ESP_OK)
    PORT_EXIT_CRITICAL();

/* restore GPIO direction because, after calling dht_fetch_data(), the
 * GPIO direction mode changes */
gpio_set_direction(pin, GPIO_MODE_OUTPUT_OD);
gpio_set_level(pin, 1);

if (result != ESP_OK)
    return result;

if (data[4] != ((data[0] + data[1] + data[2] + data[3]) & 0xFF))
{
    ESP_LOGE(TAG, "Checksum failed, invalid data received from sensor");
    return ESP_ERR_INVALID_CRC;
}

if (humidity)
    *humidity = dht_convert_data(sensor_type, data[0], data[1]);
if (temperature)
    *temperature = dht_convert_data(sensor_type, data[2], data[3]);

ESP_LOGD(TAG, "Sensor data: humidity=%d, temp=%d", *humidity, *temperature);

return ESP_OK;
}

esp_err_t dht_read_float_data(dht_sensor_type_t sensor_type, gpio_num_t pin,
    float *humidity, float *temperature)
{
CHECK_ARG(humidity || temperature);

int16_t i_humidity, i_temp;

esp_err_t res = dht_read_data(sensor_type, pin, humidity ? &i_humidity : NULL, temperature ? &i_temp : NULL);
if (res != ESP_OK)
    return res;

if (humidity)
    *humidity = i_humidity / 10.0;
if (temperature)
    *temperature = i_temp / 10.0;

return ESP_OK;
}

#define DHT_PIN GPIO_NUM_9   // DHT22的数据引脚连接GPIO9

int16_t data[2];

// 定时器回调函数
static void DHT22_timer_callback(void* arg) {
	ESP_LOGI(TAG, "Timer 1 interrupt triggered!");
	if (dht_read_data(DHT_TYPE_AM2301, DHT_PIN, &data[0], &data[1]) == ESP_OK)
	{
		printf("T: %.1f°C, H: %.1f%%", data[1] / 10.0, data[0] / 10.0);
	}
}
    
//MPU6050区域
// 假设 I2cDevice 类的定义
class I2cDevice_ {
public:
    I2cDevice_(i2c_master_bus_handle_t i2c_bus, uint8_t addr);
    ~I2cDevice_();
    void WriteReg(uint8_t reg, uint8_t value);
    uint8_t ReadReg(uint8_t reg);
    void ReadRegs(uint8_t reg, uint8_t* buffer, size_t length);

private:
    i2c_master_dev_handle_t i2c_device_;
};

I2cDevice_::I2cDevice_(i2c_master_bus_handle_t i2c_bus, uint8_t addr) {
    i2c_device_config_t i2c_device_cfg = {
       .dev_addr_length = I2C_ADDR_BIT_LEN_7,
       .device_address = addr,
       .scl_speed_hz = 400 * 1000,
       .scl_wait_us = 0,
       .flags = {
           .disable_ack_check = 0,
        },
    };
    ESP_ERROR_CHECK(i2c_master_bus_add_device(i2c_bus, &i2c_device_cfg, &i2c_device_));
    assert(i2c_device_ != NULL);
}

I2cDevice_::~I2cDevice_() {
    if (i2c_device_) {
        i2c_master_bus_rm_device(i2c_device_);
    }
}

void I2cDevice_::WriteReg(uint8_t reg, uint8_t value) {
    uint8_t buffer[2] = {reg, value};
    ESP_ERROR_CHECK(i2c_master_transmit(i2c_device_, buffer, 2, 100));
}

uint8_t I2cDevice_::ReadReg(uint8_t reg) {
    uint8_t buffer[1];
    ESP_ERROR_CHECK(i2c_master_transmit_receive(i2c_device_, &reg, 1, buffer, 1, 100));
    return buffer[0];
}

void I2cDevice_::ReadRegs(uint8_t reg, uint8_t* buffer, size_t length) {
    ESP_ERROR_CHECK(i2c_master_transmit_receive(i2c_device_, &reg, 1, buffer, length, 100));
}

// MPU6050 相关常量
const uint8_t MPU6050_ADDR = 0x68;
const uint8_t PWR_MGMT_1 = 0x6B;
const uint8_t ACCEL_XOUT_H = 0x3B;
const uint8_t GYRO_XOUT_H = 0x43;

// I2C 总线初始化
i2c_master_bus_handle_t MPU6050_InitializeI2cBus_() {
    i2c_master_bus_config_t i2c_bus_cfg = {
       .i2c_port = (i2c_port_t)1,
       .sda_io_num = GPIO_NUM_20,
       .scl_io_num = GPIO_NUM_19,
       .clk_source = I2C_CLK_SRC_DEFAULT,
       .glitch_ignore_cnt = 7,
       .intr_priority = 0,
       .trans_queue_depth = 0,
       .flags = {
           .enable_internal_pullup = 1,
        },
    };
    i2c_master_bus_handle_t i2c_bus;
    ESP_ERROR_CHECK(i2c_new_master_bus(&i2c_bus_cfg, &i2c_bus));
    return i2c_bus;
}

// 唤醒 MPU6050
void WakeUpMPU6050(I2cDevice_& mpu6050Device) {
    mpu6050Device.WriteReg(PWR_MGMT_1, 0);
}

// 获取 MPU6050 加速度计数据
void GetMPU6050AccelData(I2cDevice_& mpu6050Device, double& ax, double& ay, double& az) {
    uint8_t buffer[6];
    mpu6050Device.ReadRegs(ACCEL_XOUT_H, buffer, 6);
    ax = (buffer[0] << 8) | buffer[1];
    ay = (buffer[2] << 8) | buffer[3];
    az = (buffer[4] << 8) | buffer[5];
}

// 获取 MPU6050 陀螺仪数据
void GetMPU6050GyroData(I2cDevice_& mpu6050Device, int16_t& gx, int16_t& gy, int16_t& gz) {
    uint8_t buffer[6];
    mpu6050Device.ReadRegs(GYRO_XOUT_H, buffer, 6);
    gx = (buffer[0] << 8) | buffer[1];
    gy = (buffer[2] << 8) | buffer[3];
    gz = (buffer[4] << 8) | buffer[5];
}

// 互补滤波系数
#define ALPHA 0.98

// 全局变量用于存储角度
float pitch = 0;
float roll = 0;

// 近似 sqrt 函数
float my_sqrt(float x) {
	if (x <= 0) return 0;
	float guess = x;
	float epsilon = 0.0001; // 精度
	while (1) {
		float next_guess = 0.5 * (guess + x / guess);
		if ((next_guess - guess < epsilon) && (guess - next_guess < epsilon)) {
			break;
		}
		guess = next_guess;
	}
	return guess;
}

// 近似 atan2 函数
float my_atan2(float y, float x) {
	const float pi = 3.14159265358979323846;
	float result;
	if (x == 0) {
		if (y > 0) result = pi / 2;
		else if (y < 0) result = -pi / 2;
		else result = 0;
	}
	else {
		float atan;
		float z = y / x;
		if (z * z < 1) {
			atan = z / (1 + 0.28 * z * z);
			if (x < 0) {
				if (y < 0) result = atan - pi;
				else result = atan + pi;
			}
			else {
				result = atan;
			}
		}
		else {
			atan = pi / 2 - z / (z * z + 0.28);
			if (y < 0) result = atan - pi;
			else result = atan;
		}
	}
	return result;
}

// I2C 主设备初始化
void MPU6050_InitializeI2c() {
    i2c_master_bus_handle_t i2c_bus_;
    // Initialize I2C peripheral
    i2c_master_bus_config_t i2c_bus_cfg = {
        .i2c_port = (i2c_port_t)2,
        .sda_io_num = GPIO_NUM_20,
        .scl_io_num = GPIO_NUM_19,
        .clk_source = I2C_CLK_SRC_DEFAULT,
        .glitch_ignore_cnt = 6,
        .intr_priority = 0,
        .trans_queue_depth = 0,
        .flags = {
            .enable_internal_pullup = 1,
        },
    };
    ESP_ERROR_CHECK(i2c_new_master_bus(&i2c_bus_cfg, &i2c_bus_));

    // // Initialize XL9555
    // xl9555_ = new XL9555(i2c_bus_, 0x20);
}

// 互补滤波函数
void complementary_filter(int16_t gyro_x, int16_t gyro_y, int16_t gyro_z, int16_t accel_x, int16_t accel_y, int16_t accel_z, float dt)
{
	// 陀螺仪灵敏度，假设满量程为±2000°/s
	const float gyro_sensitivity = 16.4;

	// 计算陀螺仪角速度
	float gyro_pitch_rate = (float)gyro_x / gyro_sensitivity;
	float gyro_roll_rate = (float)gyro_y / gyro_sensitivity;

	// 计算加速度计角度
	float accel_pitch = my_atan2((float)accel_x, (float)accel_z) * 180 / 3.14159265358979323846;
	float accel_roll = my_atan2((float)accel_y, my_sqrt(accel_x * accel_x + accel_z * accel_z)) * 180 / 3.14159265358979323846;

	// 互补滤波
	pitch = ALPHA * (pitch + gyro_pitch_rate * dt) + (1 - ALPHA) * accel_pitch;
	roll = ALPHA * (roll + gyro_roll_rate * dt) + (1 - ALPHA) * accel_roll;
}

double ax, ay, az;
int16_t gx, gy, gz;
int init_num=0;

// 初始化 I2C 总线
i2c_master_bus_handle_t i2c_bus = MPU6050_InitializeI2cBus_();

// 创建 MPU6050 设备对象
I2cDevice_ mpu6050Device(i2c_bus, MPU6050_ADDR);

// 定时器回调函数
static void MPU6050_timer_callback(void* arg) {
    ESP_LOGI(TAG, "Timer 2 interrupt triggered!");
    if(init_num==0)
    {
        init_num=1;
        // 唤醒 MPU6050
WakeUpMPU6050(mpu6050Device);
    }
    if(init_num==1)
    {
        GetMPU6050AccelData(mpu6050Device, ax, ay, az);
    ESP_LOGI("MPU6050", "Accel: X=%.f, Y=%.f, Z=%.f", ax*0.00555555, ay*0.00555555, az*0.00555555);
    }
}

//SEVOR区域
#define LEDC_TIMER        LEDC_TIMER_0       // 选择 LEDC 定时器
#define LEDC_MODE         LEDC_LOW_SPEED_MODE // 低速模式（ESP32-S3 仅支持低速）

// GPIO 8 配置（1kHz, 50%）
//#define LEDC_CHANNEL_1    LEDC_CHANNEL_0
#define LEDC_GPIO_1       GPIO_NUM_8
#define LEDC_FREQ_1       1000  // 1kHz
#define LEDC_DUTY_1       (512) // 50% (1024 * 50%)

int sevor_duty=512;
int i1=0;

// 定时器回调函数
static void SEVOR_timer_callback(void* arg) {
    ESP_LOGI(TAG, "Timer 3 interrupt triggered!");
    if(i1==0)
    {
        i1=1;
        sevor_duty=512;
        ledc_set_duty(LEDC_MODE, LEDC_CHANNEL_1, sevor_duty);
	ledc_update_duty(LEDC_MODE, LEDC_CHANNEL_1);
    }
    else if(i1==1)
    {
        i1=0;
        sevor_duty=768;
        ledc_set_duty(LEDC_MODE, LEDC_CHANNEL_1, sevor_duty);
	ledc_update_duty(LEDC_MODE, LEDC_CHANNEL_1);
    }
    
}

//MOTOR区域
// GPIO 3 配置（2kHz, 25%）
//#define LEDC_CHANNEL_2    LEDC_CHANNEL_1
#define LEDC_GPIO_2       GPIO_NUM_3
#define LEDC_FREQ_2       5000  // 5kHz
#define LEDC_DUTY_2       (256) // 25% (1024 * 25%)

int motor_duty=512;
int i2=0;

// 定时器回调函数
static void MOTOR_timer_callback(void* arg) {
    ESP_LOGI(TAG, "Timer 4 interrupt triggered!");
    if(i2==0)
    {
        i2=1;
        motor_duty=512;
        ledc_set_duty(LEDC_MODE, LEDC_CHANNEL_2, motor_duty);
	ledc_update_duty(LEDC_MODE, LEDC_CHANNEL_2);
    }
    else if(i2==1)
    {
        i2=0;
        motor_duty=768;
        ledc_set_duty(LEDC_MODE, LEDC_CHANNEL_2, motor_duty);
	ledc_update_duty(LEDC_MODE, LEDC_CHANNEL_2);
    }
}

//HEATING区域
// GPIO 10 配置（5kHz, 50%）
//#define LEDC_CHANNEL_1    LEDC_CHANNEL_0
#define LEDC_GPIO_3       GPIO_NUM_10
#define LEDC_FREQ_3       5000  // 1kHz
#define LEDC_DUTY_3       (512) // 50% (1024 * 50%)

int heating_duty=512;
int i3=0;

// 定时器回调函数
static void HEATING_timer_callback(void* arg) {
    ESP_LOGI(TAG, "Timer 5 interrupt triggered!");
    if(i3==0)
    {
        i3=1;
        heating_duty=512;
        ledc_set_duty(LEDC_MODE, LEDC_CHANNEL_3, heating_duty);
	ledc_update_duty(LEDC_MODE, LEDC_CHANNEL_3);
    }
    else if(i3==1)
    {
        i3=0;
        heating_duty=768;
        ledc_set_duty(LEDC_MODE, LEDC_CHANNEL_3, heating_duty);
	ledc_update_duty(LEDC_MODE, LEDC_CHANNEL_3);
    }
}

void ledc_Init()
{
    //配置成SEVOR的频率
	ledc_timer_config_t ledc_timer = {
		.speed_mode = LEDC_MODE,
		.duty_resolution = LEDC_TIMER_10_BIT,  // 10-bit 分辨率 (0~1023)
		.timer_num = LEDC_TIMER,
		.freq_hz = LEDC_FREQ_1,        // 设置定时器初始频率
		.clk_cfg = LEDC_AUTO_CLK       // 自动选择时钟源
	};
	ledc_timer_config(&ledc_timer);
    
	ledc_channel_config_t ledc_channel1 = {
		.gpio_num = LEDC_GPIO_1,
		.speed_mode = LEDC_MODE,
		.channel = LEDC_CHANNEL_1,
		.timer_sel = LEDC_TIMER,
		.duty = LEDC_DUTY_1,
		.hpoint = 0
	};
	ledc_channel_config(&ledc_channel1);

    //配置成MOTOR的频率，HEATING的频率和MOTOR的频率一样
	// 3. 配置 GPIO3（2kHz，25% 占空比）
	ledc_timer.freq_hz = LEDC_FREQ_2;  // 修改定时器频率
	ledc_timer_config(&ledc_timer);    // 重新配置定时器

	ledc_channel_config_t ledc_channel2 = {
		.gpio_num = LEDC_GPIO_2,
		.speed_mode = LEDC_MODE,
		.channel = LEDC_CHANNEL_2,
		.timer_sel = LEDC_TIMER,
		.duty = LEDC_DUTY_2,
		.hpoint = 0
	};
	ledc_channel_config(&ledc_channel2);

    ledc_channel_config_t ledc_channel3 = {
		.gpio_num = LEDC_GPIO_3,
		.speed_mode = LEDC_MODE,
		.channel = LEDC_CHANNEL_3,
		.timer_sel = LEDC_TIMER,
		.duty = LEDC_DUTY_3,
		.hpoint = 0
	};
	ledc_channel_config(&ledc_channel3);
}

extern "C" void app_main(void)
{
    //MPU6050_InitializeI2c();

    // 初始化 MPU6050
    // esp_err_t err = mpu6050_init();
    // if (err != ESP_OK) {
    //     ESP_LOGE(TAG, "Failed to initialize MPU6050: %d", err);
    //     return;
    // }

    ledc_Init();

    // Initialize the default event loop
    ESP_ERROR_CHECK(esp_event_loop_create_default());

    // Initialize NVS flash for WiFi configuration
    esp_err_t ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_LOGW(TAG, "Erasing NVS flash to fix corruption");
        ESP_ERROR_CHECK(nvs_flash_erase());
        ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK(ret);

    // 定时器1配置结构体
	const esp_timer_create_args_t timer1_args = {
        .callback = &DHT22_timer_callback,
        // 这里可以传递参数给回调函数，若不需要则置为NULL
       .arg = NULL,
       // 定时器名称，可用于调试
      .name = "Timer1"
     };
     // 定时器2配置结构体
    const esp_timer_create_args_t timer2_args = {
        .callback = &MPU6050_timer_callback,
        .arg = NULL,
        .name = "Timer2"
     };

     // 定时器3配置结构体
    const esp_timer_create_args_t timer3_args = {
        .callback = &SEVOR_timer_callback,
        .arg = NULL,
        .name = "Timer3"
     };
 
     // 定时器4配置结构体
     const esp_timer_create_args_t timer4_args = {
        .callback = &MOTOR_timer_callback,
        .arg = NULL,
        .name = "Timer4"
     };
 
     // 定时器5配置结构体
     const esp_timer_create_args_t timer5_args = {
        .callback = &HEATING_timer_callback,
        .arg = NULL,
        .name = "Timer5"
     };
 
     esp_timer_handle_t timer1, timer2, timer3, timer4, timer5;

    //  // 创建定时器1
    //  ESP_ERROR_CHECK(esp_timer_create(&timer1_args, &timer1));
 
    //  // 启动定时器1，周期为1000000微秒（1秒）
    //  ESP_ERROR_CHECK(esp_timer_start_periodic(timer1, 1000000));

     // 创建定时器2
     ESP_ERROR_CHECK(esp_timer_create(&timer2_args, &timer2));

     // 启动定时器2，周期为1000000微秒（1秒）
     ESP_ERROR_CHECK(esp_timer_start_periodic(timer2, 1000000));

     // 创建定时器3
     ESP_ERROR_CHECK(esp_timer_create(&timer3_args, &timer3));

     // 启动定时器3，周期为1000000微秒（1秒）
     ESP_ERROR_CHECK(esp_timer_start_periodic(timer3, 5000000));

     // 创建定时器4
     ESP_ERROR_CHECK(esp_timer_create(&timer4_args, &timer4));

     // 启动定时器4，周期为1000000微秒（1秒）
     ESP_ERROR_CHECK(esp_timer_start_periodic(timer4, 1000000));

     // 创建定时器5
     ESP_ERROR_CHECK(esp_timer_create(&timer5_args, &timer5));
     
     // 启动定时器5，周期为1000000微秒（1秒）
     ESP_ERROR_CHECK(esp_timer_start_periodic(timer5, 1000000));

     ESP_LOGI(TAG, "Five timers with 1-second period started");

     // 初始化 I2C 总线
    // esp_err_t err = MPU6050_i2c_master_bus_init();
    // if (err != ESP_OK) {
    //     ESP_LOGE(TAG, "Failed to initialize I2C bus: %d", err);
    //     return;
    // }

    // Launch the application
    Application::GetInstance().Start();
    // The main thread will exit and release the stack memory
}
